#!coding:utf-8
import rospy
from sensor_msgs.msg import JointState
import numpy as np
from pupper.hardware_interface import HardwareInterface


class PupperDriverNode:
    def __init__(self):
        self.node_name = rospy.get_name()
        rospy.loginfo("[%s] Initializing......" % (self.node_name))
        self.hardware_interface = HardwareInterface()
        self.sub_joint_states = rospy.Subscriber(
            "/joint_states", JointState, self.cbJointState, queue_size=1)
        rospy.loginfo("[%s] Initialized." % (self.node_name))

    def cbJointState(self, msg):
        states = self.toConfig(msg.position)
        self.hardware_interface.set_actuator_postions(states)

    def toConfig(self, angles):
        states = np.zeros((3, 4))
        for i in range(4):
            for j in range(3):
                states[j][i] = angles[i*3+j]
        return states

    def onShutdown(self):
        # 节点关闭时要执行的操作
        rospy.loginfo("[%s] Closing ." % (self.node_name))
        rospy.loginfo("[%s] Shutdown." % (self.node_name))


if __name__ == '__main__':
    rospy.init_node('pupper_driver_node')
    node = PupperDriverNode()
    rospy.on_shutdown(node.onShutdown)
    rospy.spin()
